#include "robotsegment.h"

RobotSegment::RobotSegment(NameGroup names, Dimension3D dimensions,
                           QString description, RobotComponent *parent,
                           QVector3D offset, bool invertLength) :
    RobotComponent(SEGMENT, names, dimensions, description, parent, offset, invertLength)
{}

QVector3D RobotSegment::endPoint() const{
    bool inverted;
    float lengthTranslation;
    RobotComponent* curComponent;
    Dimension3D dimensions;
    QVector3D startPoint, offset;
    QMatrix4x4 endPointMatrix;
    QStack<RobotComponent*> componentStack = parentStack();
    const RobotComponent* topComponent = componentStack.top();

    if(topComponent == NULL){
        qDebug() << "No parents";
        startPoint = QVector3D(0,0,0);
    }
    else{
        startPoint = topComponent->offset();
    }

    //Loop through the stack (which has the root parent at the top)
    //and create a tranformation matrix by translating along the
    //lengths of the components and rotating the matrix according to
    //the joints
    while(!componentStack.empty()){
        curComponent = componentStack.pop();

        dimensions = curComponent->dimensions();
        inverted = curComponent->inverted();
        offset = curComponent->offset();

        //Translate to the offset
        endPointMatrix.translate(offset.x(), offset.y(), offset.z());

        if(curComponent->type() == SEGMENT){
            //Translate to the end of the segment (length is defined in
            //the -y direction)
            lengthTranslation = inverted ? dimensions.y : -(dimensions.y);
            endPointMatrix.translate(0, lengthTranslation);
        }
        else if(curComponent->type() == JOINT_GROUP){
            lengthTranslation = inverted ? (dimensions.y / 2) : -(dimensions.y / 2);

            //Translate to the center of the joint
            endPointMatrix.translate(0,lengthTranslation);
            //Rotate the matrix according to the joints
            endPointMatrix.rotate(((RobotJointGroup*)curComponent)->rotation());
            //Translate to the end of the joint from the center
            endPointMatrix.translate(0,lengthTranslation);
        }
    }

    //Translate to the end of this segment
    endPointMatrix.translate(_offset.x(), _offset.y(), _offset.z());
    lengthTranslation = _inverted ? _dimensions.y : -(_dimensions.y);
    endPointMatrix.translate(0, lengthTranslation);

    return endPointMatrix.map(startPoint);
}
